#include "communication/communication_plugin.h"
#include <pluginlib/class_list_macros.h>
#include <boost/any.hpp>
#include <sstream>
#include <fstream>
#include <iostream>
#include <smarteye_common/HParamSetSrv.h>
#include <stdlib.h>



namespace commplugin {

/**
 * @brief Parameter storage
 *
 * Stores parameter value.
 */
struct Parameter {
        std::string param_id;
        float param_value;
};


class ParamPlugin : public CommPlugin {
public:
        ParamPlugin() :
                param_nh("~param"),
                station_link(nullptr),
                param_state(PR_IDLE)
        { };

        void initialize(augmavconn::AugMavConnInterface::AugPtr station_link_, int target_system_id_, int target_component_id_)
        {
            station_link = station_link_;
            target_system_id = target_system_id_;
            target_component_id = target_component_id_;
            client_of_commander = param_nh.serviceClient<smarteye_common::HParamSetSrv>("/commander/host_param/set_param");
                //TO DO
                //initialize parameters


            //initialize_parameters();

//            Parameter param1 = {"CTL_HDR_KP", 1.0};
//            Parameter param2 = {"CTL_HDR_KP1", 2.0};
//            Parameter param3 = {"CTL_HDR_KD", 3.0};
//           // param_nh.getParam("/camander/CTL_HDR_KP", plugin_whitelist);

//            parameters.insert(std::pair<std::string,Parameter>(param1.param_id,param1));
//            parameters.insert(std::pair<std::string,Parameter>(param2.param_id,param2));
//            parameters.insert(std::pair<std::string,Parameter>(param3.param_id,param3));

        }

        const message_map get_rx_handlers() {
                return {
                               MESSAGE_HANDLER(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, &ParamPlugin::handle_param_request_list),
                               MESSAGE_HANDLER(MAVLINK_MSG_ID_PARAM_REQUEST_READ, &ParamPlugin::handle_param_request_read),
                               MESSAGE_HANDLER(MAVLINK_MSG_ID_PARAM_SET, &ParamPlugin::handle_param_set)
                };
        }

private:
        std::recursive_mutex mutex;
        ros::NodeHandle param_nh;
        augmavconn::AugMavConnInterface::AugPtr station_link;
        int target_system_id;
        int target_component_id;
        ros::ServiceClient client_of_commander;
        std::map<std::string, Parameter> parameters;
        enum {
                PR_IDLE,
                PR_TXPARAM,
                PR_STPARAM
        } param_state;



        void initialize_parameters()
        {
            ROS_INFO("start reading parameters from file:");
            parameters.clear();
            std::string name;
            float value;
            float value_send=0.0;
            std::fstream f;
            //std::system("pwd");
            f.open("../../include/commander/commander/host_param.txt",std::ios::in);

            if(f.is_open())
            {
                int now = f.tellg();
                f.seekg (0, f.end);
                int length = f.tellg();
                f.seekg (0, f.beg);
                while((now>=0) && (now<length-2))
                {
                    f>>name>>value;
                    std::string param_name = "/"+name;
                    param_nh.getParam(param_name, value_send);
                    Parameter param = {name, value_send};
                    parameters.insert(std::pair<std::string,Parameter>(name,param));
                    now = f.tellg();
                }
                f.close();
                ROS_INFO("Number of parameters have initialized: %d",(int)parameters.size());
            }
            else
            {
                ROS_ERROR("Communication: open parameter file failed");
            }
        }

        void handle_param_request_list(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid)
        {

            //read parameter sever
            mavlink_param_request_list_t plistMsg;
            mavlink_msg_param_request_list_decode(msg,&plistMsg);


            if (plistMsg.target_system != station_link->get_system_id()
                    || plistMsg.target_component != station_link->get_component_id())
            {
                return;
            }

            if (param_state != PR_IDLE)
            {
                return;
            }

            //start to send back all the param values


            initialize_parameters();


            unique_lock lock(mutex);

            param_state = PR_TXPARAM;

            for(auto &pair: parameters)
            {
                param_value(pair.second);
            }

            param_state = PR_IDLE;

        }

        void param_value(Parameter &param)
        {
            char param_id[sizeof(mavlink_param_value_t::param_id)];
            strncpy(param_id, param.param_id.c_str(), sizeof(param_id));

            mavlink_message_t msg;


            mavlink_msg_param_value_pack_chan(PACK_CHAN(station_link),
                                              &msg,
                                              param_id,param.param_value,
                                              MAV_PARAM_TYPE_REAL32,0,0);
            ROS_DEBUG("param_value=%f",param.param_value);
            station_link->send_message(&msg);


        }

        void handle_param_request_read(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid)
        {

            mavlink_param_request_read_t preadMsg;
            mavlink_msg_param_request_read_decode(msg,&preadMsg);

            if (preadMsg.target_system != station_link->get_system_id()
                    || preadMsg.target_component != station_link->get_component_id())
            {
                return;
            }

            if (param_state != PR_IDLE)
            {
                return;
            }

            //start to send back all the param values
            unique_lock lock(mutex);

            param_state = PR_TXPARAM;
            std::string param_id(preadMsg.param_id);

            std::map<std::string,Parameter>::iterator it = parameters.find(param_id);
            if(it == parameters.end())
            {
                ROS_DEBUG_STREAM_NAMED("param", "PR: can not find a param with id =  " << param_id);
            }
            else
            {
                param_value(it->second);

            }

            param_state = PR_IDLE;

        }

         void handle_param_set(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid)
         {
             mavlink_param_set_t psMsg;
             mavlink_msg_param_set_decode(msg, &psMsg);


             if (psMsg.target_system != station_link->get_system_id()
                     || psMsg.target_component != station_link->get_component_id())
             {
                 return;
             }


             if (param_state != PR_IDLE)
             {
                 return;
             }

             unique_lock lock(mutex);

             param_state = PR_STPARAM;



             Parameter param;
             param.param_id = psMsg.param_id;
             param.param_value = psMsg.param_value;

             ROS_INFO_STREAM_NAMED("param", "PR: handle param set with id =  " << param.param_id);

             smarteye_common::HParamSetSrv srv;
             srv.request.param_id = param.param_id;
             srv.request.param_value = param.param_value;
             srv.request.param_type = psMsg.param_type;

             client_of_commander.call(srv);

             if (srv.response.paramset_ack)
             {
                 ROS_WARN_STREAM("PR: set param success ");
                 param_value(param);

             }
             else
             {
                 ROS_INFO_NAMED("param", "PR: set param failed");
             }

             param_state = PR_IDLE;
         }


};

};//commplugin namespace

PLUGINLIB_EXPORT_CLASS(commplugin::ParamPlugin, commplugin::CommPlugin)
